#include "ps2_handle.h"


/**
 * *All of the code from the Internet*
 * Reorganize: Vaine0
 * Date: 2021/3/10
 **/
// char RxBuffer[RXBUFFERSIZE];

void PS2_Handle_test(void)
/**
 * Using the function printf() and HAL_Delay();
 **/
{
	uint8_t PS2_KEY = 0;

	PS2_KEY = PS2_DataKey();
	
	if(PS2_KEY!=0)
    {
			printf("%d is light \r\n",PS2_Handle_Data[1]);//ID
			switch(PS2_KEY)
			{
				case PSB_SELECT: 	  printf("PSB_SELECT \r\n");  break;
				case PSB_L3:     	  printf("PSB_L3 \r\n");  break;  
				case PSB_R3:        printf("PSB_R3 \r\n");  break;  
				case PSB_START:   	printf("PSB_START \r\n");  break;  
				case PSB_PAD_UP: 	  printf("PSB_PAD_UP \r\n");  break;  
				case PSB_PAD_RIGHT:	printf("PSB_PAD_RIGHT \r\n");  break;
				case PSB_PAD_DOWN:  printf("PSB_PAD_DOWN \r\n");  break; 
				case PSB_PAD_LEFT:  printf("PSB_PAD_LEFT \r\n");  break; 
				case PSB_L2:      	printf("PSB_L2 \r\n");  break;
				case PSB_R2:        printf("PSB_R2 \r\n"); break; 
				case PSB_L1:      	printf("PSB_L1 \r\n");  break; 
				case PSB_R1:      	printf("PSB_R1 \r\n");  break;     
				case PSB_TRIANGLE:	printf("PSB_TRIANGLE \r\n");  break; 							
				case PSB_CIRCLE:  	printf("PSB_CIRCLE \r\n");  break;  	
				case PSB_CROSS:   	printf("PSB_CROSS \r\n");  break; 					
				case PSB_SQUARE:  	printf("PSB_SQUARE \r\n");  break;
			}

			if(PS2_KEY == 12)// | PS2_KEY == 10)
			{
				PS2_Vibration(0xFF,0x00);
				HAL_Delay(150);
			}
			else if(PS2_KEY == 11)// | PS2_KEY == 9)
			{
				PS2_Vibration(0x00,0xFF);
				HAL_Delay(100);
			}
			else
        PS2_Vibration(0x00,0x00); 
    }
			
		printf(" %5d %5d %5d %5d\r\n",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY),
		                              PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
		HAL_Delay(100);
}

void delay_us(__IO uint32_t delay)
{
  int last, curr, val;
  int temp;

  while (delay != 0)
  {
    temp = delay > 900 ? 900 : delay;
    last = SysTick->VAL;
    curr = last - CPU_FREQUENCY_MHZ * temp;
    if (curr >= 0)
    {
      do
      {
          val = SysTick->VAL;
      }
      while ((val < last) && (val >= curr));
    }
    else
    {
      curr += CPU_FREQUENCY_MHZ * 1000;
      do
      {
          val = SysTick->VAL;
      }
      while ((val <= last) || (val > curr));
    }
    delay -= temp;
  }
}


uint16_t Handkey;
uint8_t Comd[2]={0x01,0x42};
uint8_t PS2_Handle_Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
uint8_t scan[9]={0x01,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
uint16_t MASK[]={
  PSB_SELECT,
  PSB_L3,
  PSB_R3,
  PSB_START,
  PSB_PAD_UP,
  PSB_PAD_RIGHT,
  PSB_PAD_DOWN,
  PSB_PAD_LEFT,
  PSB_L2,
  PSB_R2,
  PSB_L1,
  PSB_R1 ,
  PSB_GREEN,
  PSB_RED,
  PSB_BLUE,
  PSB_PINK,
  PSB_TRIANGLE,
  PSB_CIRCLE,
  PSB_CROSS,
  PSB_SQUARE
};


void PS2_Init(void)
/**
 * ps2 handle init and config;
 **/
{
  DO_H;
  CLK_H;
  CS_H;
  
  uint8_t short_poll[9]={0x01,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
  uint8_t enter_config[9]={0x01,0x43,0x00,0x01,0x00,0x00,0x00,0x00,0x00};
  uint8_t config[9]={0x01,0x44,0x00,0x01/*0x00*/,0x03/*0xEE*/,0x00,0x00,0x00,0x00};
  uint8_t vibration_on[5]={0x01,0x4D,0x00,0x00,0x01};
  uint8_t exit_config[9]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A};
  PS2_ReadData(short_poll, 9);
  PS2_ReadData(short_poll, 9);
  PS2_ReadData(short_poll, 9);
  PS2_ReadData(enter_config, 9);
  PS2_ReadData(config, 9);
  PS2_ReadData(vibration_on, 5);
  PS2_ReadData(exit_config, 9);
}


uint8_t PS2_ReadByte(uint8_t command)
{
  uint8_t i,j=1;
  uint8_t res=0; 
  for(i=0; i<=7; i++)          
  {
  if(command&0x01)
    DO_H;
  else
    DO_L;
  command = command >> 1;
  delay_us(10);//10
  CLK_L;
  delay_us(10); //10
  if(DI) 
    res = res + j;
  j = j << 1; 
  CLK_H;
  delay_us(10);//10	 
  }
  DO_H;
  delay_us(55);//50
  return res;	
}

void PS2_ReadData(uint8_t *command, uint8_t size)
{
  uint8_t i;
  CS_L;
  delay_us(6);
  for(i=0; i<size; i++)
  {
    PS2_Handle_Data[i] = PS2_ReadByte(command[i]);
  }
  CS_H;
  delay_us(6);
}


unsigned char PS2_DataKey()
/**
 * **Mapping table is defined in ps2_handle.h**
 * return 0, no key was pressed;
 * else, the value is the pressed key;
 **/
{
	uint8_t index = 0;

	PS2_ClearData();
	PS2_ReadData(scan, 9);

	Handkey=(PS2_Handle_Data[4]<<8)|PS2_Handle_Data[3];
	for(index=0;index<16;index++)
	{	    
		if((Handkey&(1<<(MASK[index]-1)))==0)
			return index+1;
	}
	return 0;
}

uint16_t PS2_KeyData()
{
  Handkey = (PS2_Handle_Data[4] << 8) | PS2_Handle_Data[3];
  return Handkey;
}


uint8_t PS2_RedLight(void)
/**
 * Get the mode of ps2 handle;
 * return 0, it's red light mode;
 * return 1, it's green light mode or connection fail;
 **/
{
	// CS_L;
	// PS2_ReadByte(Comd[0]);
	// PS2_ReadByte(Comd[1]);
	// CS_H;
	// // if( PS2_Handle_Data[1] == 0X73)   return 0 ;    // 红灯
  // // else if (PS2_Handle_Data[1] == 0x41) return 1;  // 绿灯
	// // else return -1;                                 // 未连接
  PS2_ReadData(Comd, 2);
  return PS2_Handle_Data[1];
}

uint8_t PS2_Light(void)
{
  if (PS2_Handle_Data[1] == 0x73)       
    return 0;
  else if (PS2_Handle_Data[1] == 0x41)  
    return 1;
  else                                  
    return -1;
}

uint8_t PS2_AnologData(uint8_t button)
/**
 * Data of joystick;
 * PSS_RX;
 * PSS_RY;
 * PSS_LX;
 * PSS_LY;
 **/
{
	return PS2_Handle_Data[button];
}


void PS2_ClearData()
{
  uint8_t a;
  for(a=0;a<9;a++)
    PS2_Handle_Data[a]=0x00;
}


void PS2_Vibration(uint8_t motor1, uint8_t motor2)
/**
 * **There has to be a delay after vibration (>100ms)**
 * motor1: right motor, 0x00->OFF, else->ON;
 * motor2: left motor, 0x40 < motor2 < 0xFF;
 **/
{
  uint8_t vibration[9]={0x01,0x42,0x00,motor1,motor2,0x00,0x00,0x00,0x00};
  PS2_ReadData(vibration, 9);
}
